#!/usr/bin/env python

# Copyright (c) 2018 Intel Labs.
# authors: German Ros (german.ros@intel.com)
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.

"""Module with auxiliary functions."""

import math

import carla
import numpy as np


def draw_waypoints(world, waypoints, z=0.5):
    """Draw a list of waypoints at a certain given height (z).

    Args:
        world: carla.World object
        waypoints: list or iterable container with the waypoints to draw
        z: height in meters

    Returns:
        N/A.
    """
    for w in waypoints:
        t = w.transform
        begin = t.location + carla.Location(z=z)
        angle = math.radians(t.rotation.yaw)
        end = begin + carla.Location(x=math.cos(angle), y=math.sin(angle))
        world.debug.draw_arrow(begin, end, arrow_size=0.3, life_time=1.0)


def get_speed(vehicle):
    """Compute speed of a vehicle in Kmh.

    Args:
        vehicle: the vehicle for which speed is calculated

    Returns:
        Speed value as a float in Kmh.
    """
    vel = vehicle.get_velocity()
    return 3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)


def is_within_distance_ahead(
    target_location, current_location, orientation, max_distance
):
    """Check if a target object is within a certain distance in front of a reference object.

    Args:
        target_location: location of the target object
        current_location: location of the reference object
        orientation: orientation of the reference object
        max_distance: maximum allowed distance

    Returns:
        boolean true if target object is within max_distance ahead of the reference object.
    """
    target_vector = np.array(
        [target_location.x - current_location.x, target_location.y - current_location.y]
    )
    norm_target = np.linalg.norm(target_vector)
    if norm_target > max_distance:
        return False

    forward_vector = np.array(
        [math.cos(math.radians(orientation)), math.sin(math.radians(orientation))]
    )
    d_angle = math.degrees(
        math.acos(np.dot(forward_vector, target_vector) / norm_target)
    )

    return d_angle < 90.0


def compute_magnitude_angle(target_location, current_location, orientation):
    """Compute relative angle and distance between a target_location and a current_location.

    Args:
        target_location: location of the target object
        current_location: location of the reference object
        orientation: orientation of the reference object

    Returns:
        A tuple composed by the distance to the object and the angle between both objects.
    """
    target_vector = np.array(
        [target_location.x - current_location.x, target_location.y - current_location.y]
    )
    norm_target = np.linalg.norm(target_vector)

    forward_vector = np.array(
        [math.cos(math.radians(orientation)), math.sin(math.radians(orientation))]
    )
    d_angle = math.degrees(
        math.acos(np.dot(forward_vector, target_vector) / norm_target)
    )

    return (norm_target, d_angle)


def distance_vehicle(waypoint, vehicle_transform):
    """Compute the distance from a point to a vehicle position.

    Args:
        waypoint: path carla.Waypoint object
        vehicle_transform: carla.Transform object

    Returns:

    """
    loc = vehicle_transform.location
    dx = waypoint.transform.location.x - loc.x
    dy = waypoint.transform.location.y - loc.y

    return math.sqrt(dx * dx + dy * dy)


def vector(location_1, location_2):
    """Returns the unit vector from location_1 to location_2.

    Args:
        location_1: carla.Location objects
        location_2: carla.Location objects

    Returns:
        Norm vector between two coordinate locations.

    """
    x = location_2.x - location_1.x
    y = location_2.y - location_1.y
    z = location_2.z - location_1.z
    norm = np.linalg.norm([x, y, z])

    return [x / norm, y / norm, z / norm]
